
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode.c
  * @author     baiyang
  * @date       2021-8-12
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"
#include "mode.h"

#include <parameter/param.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool is_taking_off(mode_base_t mode);
static void bind_function(mode_base_t mode, mode_ops_t ops);
/*----------------------------------variable----------------------------------*/
static UserTakeOff takeoff;
static AutoYaw auto_yaw;

// altitude above-ekf-origin below which auto takeoff does not control horizontal position
static bool auto_takeoff_no_nav_active;
static float auto_takeoff_no_nav_alt_cm;

// auto takeoff variables
static bool auto_takeoff_no_nav_active = false;
static float auto_takeoff_no_nav_alt_cm = 0;
static float auto_takeoff_start_alt_cm = 0;
static float auto_takeoff_complete_alt_cm = 0;
static bool auto_takeoff_terrain_alt = false;
static bool auto_takeoff_complete = false;
static Vector3p auto_takeoff_complete_pos;

static struct mode_ops mode_base_ops = {
        .mode_number = NULL,
        .init        = NULL,
        .exit        = NULL,
        .run         = NULL,
        .requires_GPS = NULL,
        .has_manual_throttle = NULL,
        .allows_arming = NULL,
        .is_autopilot = NULL,
        .has_user_takeoff = NULL,
        .in_guided_mode = NULL,
        .logs_attitude = NULL,
        .allows_save_trim = NULL,
        .allows_autotune = NULL,
        .allows_flip = NULL,
        .name = NULL,
        .name4 = NULL,
        .is_taking_off = is_taking_off,
        .is_landing = NULL,
        .requires_terrain_failsafe = NULL,
        .get_wp = NULL,
        .wp_bearing = NULL,
        .wp_distance = NULL,
        .crosstrack_error = NULL,
        .output_to_motors = NULL,
        .use_pilot_yaw = NULL,
        .throttle_hover = NULL,
        .do_user_takeoff_start = NULL,
        .pause = NULL,
        .resume = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// 构造函数
void mode_ctor(mode_base_t mode, mode_ops_t ops)
{
    bind_function(mode, ops);

    mode->ahrs        = fms.ahrs;
    mode->pos_control = fms.pos_control;
    mode->attitude_control = fms.attitude_control;
    mode->loiter_nav = fms.loiter_nav;
    mode->wp_nav     = fms.wp_nav;
    mode->motors = fms.motors;
    mode->arming = &fms.arming;

    mode->channel_roll = fms.channel_roll;
    mode->channel_pitch = fms.channel_pitch;
    mode->channel_throttle = fms.channel_throttle;
    mode->channel_yaw = fms.channel_yaw;

    mode->takeoff = &takeoff;
    mode->auto_yaw = &auto_yaw;
    mode->auto_takeoff_no_nav_active = &auto_takeoff_no_nav_active;
    mode->auto_takeoff_no_nav_alt_cm = &auto_takeoff_no_nav_alt_cm;

    mode->auto_takeoff_complete = &auto_takeoff_complete;
}

static void bind_function(mode_base_t mode, mode_ops_t ops)
{
    mode->ops = &mode_base_ops;

    if (ops->is_taking_off == NULL) {
        ops->is_taking_off = mode->ops->is_taking_off;
    }

    mode->ops = ops;
}

bool mode_is_disarmed_or_landed()
{
    if (!fms.motors->_armed || !fms.ap.auto_armed || fms.ap.land_complete) {
        return true;
    }
    return false;
}

void mode_zero_throttle_and_relax_ac(bool spool_up)
{
    if (spool_up) {
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);
    } else {
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_GROUND_IDLE);
    }
    attctrl_input_euler_angle_roll_pitch_euler_rate_yaw(fms.attitude_control, 0.0f, 0.0f, 0.0f);
    attctrl_set_throttle_out(fms.attitude_control,0.0f, false, fms.g.throttle_filt);
}

void mode_zero_throttle_and_hold_attitude()
{
    // run attitude controller
    attctrl_input_euler_angle_roll_pitch_euler_rate_yaw(fms.attitude_control, 0.0f, 0.0f, 0.0f);
    attctrl_set_throttle_out(fms.attitude_control,0.0f, false, fms.g.throttle_filt);
}

AltHoldModeState mode_get_alt_hold_state(float target_climb_rate_cms)
{
    // Alt Hold State Machine Determination
    if (!fms.motors->_armed) {
        // the aircraft should moved to a shut down state
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_SHUT_DOWN);

        // transition through states as aircraft spools down
        switch (fms.motors->_spool_state) {

        case MOTOR_SHUT_DOWN:
            return AltHold_MotorStopped;

        case MOTOR_GROUND_IDLE:
            return AltHold_Landed_Ground_Idle;

        default:
            return AltHold_Landed_Pre_Takeoff;
        }

    } else if (takeoff_running(&takeoff) || takeoff_triggered(target_climb_rate_cms)) {
        // the aircraft is currently landed or taking off, asking for a positive climb rate and in THROTTLE_UNLIMITED
        // the aircraft should progress through the take off procedure
        return AltHold_Takeoff;

    } else if (!fms.ap.auto_armed || fms.ap.land_complete) {
        // the aircraft is armed and landed
        if (target_climb_rate_cms < 0.0f && !fms.ap.using_interlock) {
            // the aircraft should move to a ground idle state
            Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_GROUND_IDLE);

        } else {
            // the aircraft should prepare for imminent take off
            Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);
        }

        if (fms.motors->_spool_state == MOTOR_GROUND_IDLE) {
            // the aircraft is waiting in ground idle
            return AltHold_Landed_Ground_Idle;

        } else {
            // the aircraft can leave the ground at any time
            return AltHold_Landed_Pre_Takeoff;
        }

    } else {
        // the aircraft is in a flying state
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);
        return AltHold_Flying;
    }

}

void mode_set_throttle_takeoff()
{
    // initialise the vertical position controller
    posctrl_init_z_controller(fms.pos_control);
}

// handle situations where the vehicle is on the ground waiting for takeoff
// force_throttle_unlimited should be true in cases where we want to keep the motors spooled up
// (instead of spooling down to ground idle).  This is required for tradheli's in Guided and Auto
// where we always want the motor spooled up in Guided or Auto mode.  Tradheli's main rotor stops 
// when spooled down to ground idle.
// ultimately it forces the motor interlock to be obeyed in auto and guided modes when on the ground.
// default, force_throttle_unlimited = false.
void mode_make_safe_ground_handling(mode_base_t mode, bool force_throttle_unlimited)
{
    if (force_throttle_unlimited) {
        // keep rotors turning 
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);
    } else {
        // spool down to ground idle
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_GROUND_IDLE);
    }


    switch (fms.motors->_spool_state) {

    case MOTOR_SHUT_DOWN:
    case MOTOR_GROUND_IDLE:
        // relax controllers during idle states
        attctrl_reset_yaw_target_and_rate(fms.attitude_control, true);
        attctrl_reset_rate_controller_I_terms(fms.attitude_control);
        break;

    case MOTOR_SPOOLING_UP:
    case MOTOR_THROTTLE_UNLIMITED:
    case MOTOR_SPOOLING_DOWN:
        // while transitioning though active states continue to operate normally
        break;
    }

    posctrl_relax_z_controller(mode->pos_control, 0.0f);   // forces throttle output to decay to zero
    posctrl_update_z_controller(mode->pos_control);
    // we may need to move this out
    attctrl_input_euler_angle_roll_pitch_euler_rate_yaw(fms.attitude_control,0.0f, 0.0f, 0.0f);
}

void mode_land_run_vertical_control(mode_base_t mode, bool pause_descent)
{
    float cmb_rate = 0;
    bool ignore_descent_limit = false;
    if (!pause_descent) {

        // do not ignore limits until we have slowed down for landing
        ignore_descent_limit = (MAX(fms.g.land_alt_low,100) > mode_get_alt_above_ground_cm(mode)) || fms.ap.land_complete_maybe;

        float max_land_descent_velocity;
        if (fms.g.land_speed_high > 0) {
            max_land_descent_velocity = -fms.g.land_speed_high;
        } else {
            max_land_descent_velocity = posctrl_get_max_speed_down_cms(mode->pos_control);
        }

        // Don't speed up for landing.
        max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(fms.g.land_speed));

        // Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low.
        cmb_rate = control_sqrt_controller(MAX(fms.g.land_alt_low,100)-mode_get_alt_above_ground_cm(mode), posctrl_get_pos_z_p(mode->pos_control)->_kp, posctrl_get_max_accel_z_cmss(mode->pos_control), fms.psc_dt);

        // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed.
        cmb_rate = math_constrain_float(cmb_rate, max_land_descent_velocity, -abs(fms.g.land_speed));
    }

    // update altitude target and call position controller
    posctrl_land_at_climb_rate_cm(mode->pos_control, cmb_rate, ignore_descent_limit);
    posctrl_update_z_controller(mode->pos_control);
}

void mode_land_run_horizontal_control(mode_base_t mode)
{
    float target_roll = 0.0f;
    float target_pitch = 0.0f;
    float target_yaw_rate = 0;

    // relax loiter target if we might be landed
    if (fms.ap.land_complete_maybe) {
        loiter_soften_for_landing(mode->loiter_nav);
    }

    // process pilot inputs
    if (!fms.failsafe.radio) {
        if ((fms.g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && lpf_get_output(&fms.rc_throttle_control_in_filter) > LAND_CANCEL_TRIGGER_THR){
            //AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT);
            // exit land if throttle is high
            if (!fms_set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
                fms_set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
            }
        }

        if (fms.g.land_repositioning) {
            // apply SIMPLE mode transform to pilot inputs
            //update_simple_mode();

            // convert pilot input to lean angles
            fms_get_pilot_desired_lean_angles(&target_roll, &target_pitch, loiter_get_angle_max_cd(mode->loiter_nav), attctrl_get_althold_lean_angle_max_cd(mode->attitude_control));

            // record if pilot has overridden roll or pitch
            if (!math_flt_zero(target_roll) || !math_flt_zero(target_pitch)) {
                if (!fms.ap.land_repo_active) {
                    //AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE);
                }
                fms.ap.land_repo_active = true;
            }
        }

        // get pilot's desired yaw rate
        target_yaw_rate = fms_get_pilot_desired_yaw_rate(RC_norm_input_dz(fms.channel_yaw));
        if (!math_flt_zero(target_yaw_rate)) {
            autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);
        }
    }

    // process roll, pitch inputs
    loiter_set_pilot_desired_acceleration(mode->loiter_nav, target_roll, target_pitch);

    // run loiter controller
    loiter_update(mode->loiter_nav, true);

    Vector3f_t thrust_vector = loiter_get_thrust_vector(mode->loiter_nav);

    if (fms.g.wp_navalt_min > 0) {
        // user has requested an altitude below which navigation
        // attitude is limited. This is used to prevent commanded roll
        // over on landing, which particularly affects helicopters if
        // there is any position estimate drift after touchdown. We
        // limit attitude to 7 degrees below this limit and linearly
        // interpolate for 1m above that
        const float attitude_limit_cd = math_linear_interpolate(700, fms.g.angle_max, mode_get_alt_above_ground_cm(mode),
                                                     fms.g.wp_navalt_min*100U, (fms.g.wp_navalt_min+1)*100U);
        const float thrust_vector_max = sinf(radians(attitude_limit_cd / 100.0f)) * GRAVITY_MSS * 100.0f;
        const float thrust_vector_mag = vec3_length_xy(&thrust_vector);
        if (thrust_vector_mag > thrust_vector_max) {
            float ratio = thrust_vector_max / thrust_vector_mag;
            thrust_vector.x *= ratio;
            thrust_vector.y *= ratio;

            // tell position controller we are applying an external limit
            posctrl_set_externally_limited_xy(mode->pos_control);
        }
    }

    // call attitude controller
    if (mode->auto_yaw->_mode == AUTO_YAW_HOLD) {
        // roll & pitch from waypoint controller, yaw rate from pilot
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &thrust_vector, target_yaw_rate);
    } else {
        // roll, pitch from waypoint controller, yaw heading from auto_heading()
        attctrl_input_thrust_vector_heading(mode->attitude_control, &thrust_vector, autoyaw_yaw(mode->auto_yaw), 0.0f);
    }
}

/*
  get a height above ground estimate for landing
 */
int32_t mode_get_alt_above_ground_cm(mode_base_t mode)
{
    int32_t alt_above_ground_cm;
    //if (copter.get_rangefinder_height_interpolated_cm(alt_above_ground_cm)) {
    //    return alt_above_ground_cm;
    //}
    if (!posctrl_is_active_xy(mode->pos_control)) {
        return fms.ahrs->curr_loc.alt;
    }
    if (location_get_alt_cm(&fms.ahrs->curr_loc, ALT_FRAME_ABOVE_TERRAIN, &alt_above_ground_cm)) {
        return alt_above_ground_cm;
    }

    // Assume the Earth is flat:
    return fms.ahrs->curr_loc.alt;
}

// run normal or precision landing (if enabled)
// pause_descent is true if vehicle should not descend
// default, pause_descent = false
void mode_land_run_normal_or_precland(mode_base_t mode, bool pause_descent)
{
    mode_land_run_horiz_and_vert_control(mode, pause_descent);
}

void mode_auto_takeoff_start(mode_base_t mode, float complete_alt_cm, bool terrain_alt)
{
    auto_takeoff_start_alt_cm = mode->ahrs->relpos_cm.z;
    auto_takeoff_complete_alt_cm = complete_alt_cm;
    auto_takeoff_terrain_alt = terrain_alt;
    auto_takeoff_complete = false;
    if ((fms.g.wp_navalt_min > 0) && (mode_is_disarmed_or_landed() || !mode->motors->_interlock)) {
        // we are not flying, climb with no navigation to current alt-above-ekf-origin + wp_navalt_min
        auto_takeoff_no_nav_alt_cm = auto_takeoff_start_alt_cm + fms.g.wp_navalt_min * 100;
        auto_takeoff_no_nav_active = true;
    } else {
        auto_takeoff_no_nav_active = false;
    }
}

void mode_auto_takeoff_run(mode_base_t mode)
{
    // if not armed set throttle to zero and exit immediately
    if (!mode->motors->_armed || !fms.ap.auto_armed) {
        // do not spool down tradheli when on the ground with motor interlock enabled
        // is_tradheli() && motors->get_interlock()
        mode_make_safe_ground_handling(mode, false);
        return;
    }

    // get terrain offset
    float terr_offset = 0.0f;
    if (auto_takeoff_terrain_alt && !wpnav_get_terrain_offset(mode->wp_nav, &terr_offset)) {
        mavproxy_send_statustext(MAV_SEVERITY_CRITICAL, "auto takeoff: failed to get terrain offset");
        return;
    }

    // set motors to full range
    Motors_set_desired_spool_state(mode->motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

    // process pilot's yaw input
    float target_yaw_rate = 0;
    if (!fms.failsafe.radio && mode_use_pilot_yaw(fms.flightmode)) {
        // get pilot's desired yaw rate
        target_yaw_rate = fms_get_pilot_desired_yaw_rate(RC_norm_input_dz(mode->channel_yaw));
        if (!math_flt_zero(target_yaw_rate)) {
            autoyaw_set_mode(&auto_yaw, AUTO_YAW_HOLD);
        }
    }

    // aircraft stays in landed state until rotor speed run up has finished
    if (mode->motors->_spool_state == MOTOR_THROTTLE_UNLIMITED) {
        fms_set_land_complete(false);
    } else {
        // motors have not completed spool up yet so relax navigation and position controllers
        posctrl_relax_velocity_controller_xy(mode->pos_control);
        posctrl_update_xy_controller(mode->pos_control);
        posctrl_relax_z_controller(mode->pos_control, 0.0f);   // forces throttle output to decay to zero
        posctrl_update_z_controller(mode->pos_control);
        attctrl_reset_yaw_target_and_rate(mode->attitude_control, true);
        attctrl_reset_rate_controller_I_terms(mode->attitude_control);
        Vector3f_t thrust_vector = posctrl_get_thrust_vector(mode->pos_control);
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &thrust_vector, autoyaw_rate_cds(&auto_yaw));
        return;
    }

    // check if we are not navigating because of low altitude
    if (auto_takeoff_no_nav_active) {
        // check if vehicle has reached no_nav_alt threshold
        if (mode->ahrs->relpos_cm.z >= auto_takeoff_no_nav_alt_cm) {
            auto_takeoff_no_nav_active = false;
        }
        posctrl_relax_velocity_controller_xy(mode->pos_control);
    } else {
        Vector2f_t vel = VECTOR2F_DEFAULT_VALUE;
        Vector2f_t accel = VECTOR2F_DEFAULT_VALUE;
        posctrl_input_vel_accel_xy(mode->pos_control, &vel, &accel, true);
    }
    posctrl_update_xy_controller(mode->pos_control);

    // command the aircraft to the take off altitude
    float pos_z = auto_takeoff_complete_alt_cm + terr_offset;
    float vel_z = 0.0;
    posctrl_input_pos_vel_accel_z(mode->pos_control, &pos_z, &vel_z, 0.0, true);
    
    // run the vertical position controller and set output throttle
    posctrl_update_z_controller(mode->pos_control);

    Vector3f_t posctrl_thrust_vector = posctrl_get_thrust_vector(mode->pos_control);

    // call attitude controller
    if ((enum autopilot_yaw_mode)auto_yaw._mode == AUTO_YAW_HOLD) {
        // roll & pitch from position controller, yaw rate from pilot
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &posctrl_thrust_vector, target_yaw_rate);
    } else if ((enum autopilot_yaw_mode)auto_yaw._mode == AUTO_YAW_RATE) {
        // roll & pitch from position controller, yaw rate from mavlink command or mission item
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &posctrl_thrust_vector, autoyaw_rate_cds(&auto_yaw));
    } else {
        // roll & pitch from position controller, yaw heading from GCS or auto_heading()
        attctrl_input_thrust_vector_heading(mode->attitude_control, &posctrl_thrust_vector, autoyaw_yaw(&auto_yaw), autoyaw_rate_cds(&auto_yaw));
    }

    // handle takeoff completion
    bool reached_altitude = (posctrl_get_pos_target_z_cm(mode->pos_control) - auto_takeoff_start_alt_cm) >= ((auto_takeoff_complete_alt_cm + terr_offset - auto_takeoff_start_alt_cm) * 0.90);
    bool reached_climb_rate = posctrl_get_vel_desired_cms(mode->pos_control).z < posctrl_get_max_speed_up_cms(mode->pos_control) * 0.1f;
    auto_takeoff_complete = reached_altitude && reached_climb_rate;

    // calculate completion for location in case it is needed for a smooth transition to wp_nav
    if (auto_takeoff_complete) {
        auto_takeoff_complete_pos.x = posctrl_get_pos_target_cm(mode->pos_control).x;
        auto_takeoff_complete_pos.y = posctrl_get_pos_target_cm(mode->pos_control).y;
        auto_takeoff_complete_pos.z = posctrl_get_pos_target_cm(mode->pos_control).z;
    }

}

// initiate user takeoff - called when MAVLink TAKEOFF command is received
bool mode_do_user_takeoff(mode_base_t mode, float takeoff_alt_cm, bool must_navigate)
{
    if (!mode->motors->_armed) {
        return false;
    }
    if (!fms.ap.land_complete) {
        // can't takeoff again!
        return false;
    }
    if (!mode_has_user_takeoff(mode, must_navigate)) {
        // this mode doesn't support user takeoff
        return false;
    }
    if (takeoff_alt_cm <= fms.ahrs->curr_loc.alt) {
        // can't takeoff downwards...
        return false;
    }

    // Vehicles using motor interlock should return false if motor interlock is disabled.
    // Interlock must be enabled to allow the controller to spool up the motor(s) for takeoff.
    if (!mode->motors->_interlock && fms.ap.using_interlock) {
        return false;
    }

    if (!mode_do_user_takeoff_start(mode, takeoff_alt_cm)) {
        return false;
    }

    fms_set_auto_armed(true);
    return true;
}

static bool is_taking_off(mode_base_t mode)
{
    if (!mode_has_user_takeoff(mode, false)) {
        return false;
    }
    if (fms.ap.land_complete) {
        return false;
    }
    return takeoff_running(mode->takeoff);
}

/*------------------------------------test------------------------------------*/


